
// #include <geographic_msgs/GeoPoseStamped.h>
// #include <geometry_msgs/TwistStamped.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cstring>
#include <sstream>
#include <thread>
#include <cstdint> // For standard integer types
#include <cmath>   // For math functions// For SetMode service header

// #include <rclcpp/rclcpp.hpp>
// #include <sensor_msgs/msg/nav_sat_fix.hpp>
// #include <sensor_msgs/msg/imu.hpp>
// #include <geometry_msgs/msg/pose_stamped.hpp>
// #include <geographic_msgs/msg/geo_pose_stamped.hpp>
// #include <mavros_msgs/msg/vfr_hud.hpp>
// #include <mavros_msgs/srv/set_mode.hpp>
// #include <functional>
// #include <memory>
// #include <thread>
// #include <mutex>
// Define UDP parameters
//#define UDP_TARGET_IP "192.168.16.154"     // Single-target IP address



#define UDP_TARGET_IP "255.255.255.255" 
#define UDP_PORT 8888                       // UDP data packet send port
#define UDP_RECEIVE_PORT 8899               // UDP data packet receive port



#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geographic_msgs/msg/geo_pose_stamped.hpp>
#include <mavros_msgs/msg/vfr_hud.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "yolo_detect_message/msg/geolocation.hpp"
#include <functional>
#include <memory>
#include <thread>
#include <mutex>
#include <iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>

struct FCUCMD {
    double RefLatitude_deg;
    double RefLongitude_deg;
    double RefHeight_meter;
    double RefHdg_deg;
    double RefAirSpd_mps;
    double RefGndSpd_mps;
} __attribute__((packed));
struct uav_pos_raw {
    uint8_t seq;
    uint16_t year;
    uint8_t month;
    uint8_t day;
    uint8_t hour;
    uint8_t minute;
    uint16_t sec;
    int32_t latitude;
    int32_t longitude;
    int32_t altitude;
    int16_t pres;
    int16_t air_vel;
    int16_t ground_vel;
    int16_t roll;
    int16_t pitch;
    int16_t yaw;
    uint16_t rpm;
    uint16_t board_vol;
    uint8_t fs1;
    uint8_t fs2;
    uint8_t bd1;
    uint8_t bd2;
    uint8_t reserved[8];
    uint8_t back_type;
    uint8_t back_subtype;
    uint8_t back_verify;
} __attribute__((packed));




class UavUdpComm : public rclcpp::Node {
public:
    UavUdpComm();

    ~UavUdpComm() ;

private:
    // bool setOffboardMode() ;
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
// IMU回调函数
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) ;

    void udp_receive_thread() ;
    void setup_udp_communication();

    void start_udp_send_thread();
    void udp_send_thread();

    void send_udp_data() ;
    void state_callback(const mavros_msgs::msg::State::SharedPtr msg);
    void arm_drone();

    void set_offboard_mode();
private:

    int sockfd_;
    struct sockaddr_in destAddr_;
    std::mutex data_mutex_;

    int vehcile_id_;

    std::string namespaces;

    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
    rclcpp::Publisher<geographic_msgs::msg::GeoPoseStamped>::SharedPtr pose_publisher_;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arm_client_;
    rclcpp::Publisher<yolo_detect_message::msg::Geolocation>::SharedPtr yolo_publisher_;
    rclcpp::Time last_request_;
    
    // 当前无人机的状态信息
    mavros_msgs::msg::State current_state_;

    std::thread send_thread_;
    std::thread receive_thread_;
    uav_pos_raw data;
    std::mutex udp_mutex;  // Mutex for shared data protection
    std::mutex data_mutex;
    uint8_t seq;
    // UDP socket variables
    int sockfd, receiveSockfd;
    struct sockaddr_in destAddr, receiveAddr;
    socklen_t addrLen = sizeof(receiveAddr);
    
    bool init_imu_received = false;
    bool init_gps_received = false;

    std::string uav_namespaces = "/uav_swarms";

};

